#include "Timer.h"
#include "TPSN.h"
#include "Atm128Interrupt.h"
#include "printf.h" // ignore red line its not an error

module TPSNC {
	uses {
	    interface Leds;
	    interface Boot;
	    interface Receive;
	    interface AMSend;
	    interface PacketTimeStamp<TMicro, uint32_t> as TimeStamp;
	    interface LocalTime<TMicro> as MicroTimer;
	    interface Timer<TMilli> as MilliTimer;
	    interface Timer<TMilli> as StartTimer;
	    interface Timer<TMilli> as TimerLockLevel;
	    interface Timer<TMilli> as SyncWait;
	    interface Timer<TMilli> as SynchronizationTimer;
	    interface SplitControl as AMControl;
	    interface Packet;
	    interface Random;
	    interface ParameterInit<uint16_t> as Seed;
	    
	    // External interrupt
	    interface HplAtm128Interrupt;
  	}	
}

implementation {	
	// Prototypes
	void WaitRandom(uint32_t waitTimelow, uint32_t waitTimehigh);
	void SendMessage(nx_int8_t messageType, nx_int16_t nodeLevel, nx_int32_t t2, nx_int32_t t3);
	
	// Global variables
	message_t packet;
	Package msg_received;
	bool locked;
	bool levelLocked = FALSE;
	uint16_t currentState = StartState;
	uint16_t treeLevel = 255;
	uint16_t treeLevelReceived = 255;
	uint8_t levelDiscCount = 0;
	bool AckReceived = TRUE;
	uint32_t T1, T2, T3, T4, clkOffSet, propDelay;
	
	//Test Variables
	uint16_t test_cnt = 0;
	bool ROOTNODE = FALSE;
	uint32_t testvar;
	
  	event void Boot.booted() {
    	call AMControl.start();
    	
    	// Set up external interrupt
    	call HplAtm128Interrupt.disable();
		call HplAtm128Interrupt.clear();
		call HplAtm128Interrupt.edge(TRUE); // edge triggered, low to high
		call HplAtm128Interrupt.enable();
  	}
	
  	event void AMControl.startDone(error_t err) {
	    if (err == SUCCESS) {
	       call MilliTimer.startPeriodic(1000);
	       call StartTimer.startOneShot(200);
	  			
	       locked = FALSE; 						// unlocks the Radio send function
	    }
	    else {
	      call AMControl.start();
	    }
  	}
  
  	event void AMControl.stopDone(error_t err) {
    	// do nothing
  	}
    
  	//Function for random delay
  	void WaitRandom(uint32_t waitTimelow, uint32_t waitTimehigh) {
		call Seed.init(call MilliTimer.getNow()); // Need to seed the random function to get a true random value (any timer will do)
		call SyncWait.startOneShot((call Random.rand32() % (waitTimehigh - waitTimelow) + waitTimelow)); // Waits between 0 and WAIT_TIME ms.			
  	}
    
  	void SendMessage(nx_int8_t messageType, nx_int16_t nodeLevel, nx_int32_t t2, nx_int32_t t3) {
		if (!locked) {		
			Package * rcm; 
			rcm = (Package * )(call Packet.getPayload(&packet, TOSH_DATA_LENGTH));
		  	rcm->t2 = T2;
		  	rcm->t3 = T3;
		  	rcm->messageType = messageType;
		  	rcm->nodeLevel = nodeLevel;
		  	if (call AMSend.send(AM_BROADCAST_ADDR, &packet, TOSH_DATA_LENGTH) == SUCCESS) {
				locked = TRUE;
				printf("AMSend Succces\n");
  				printfflush();
			}else{
					printf("AMSend Failed\n");
  					printfflush();
				}
		}else{
			printf("Level still locked\n");
  			printfflush();
		}	
  	}
  
  	event void MilliTimer.fired() {
  		//Test only
  		/*if (currentState == ChildState){
  			printf("State:ChildState\n");
  			printfflush();
  		}else if (currentState == RootState){
  			printf("State:RootState\n");
  			printfflush();
  		}else if (currentState == StartState){
  			printf("State:StartState\n");
  			printfflush();
  		}else if (currentState == RootStateSendAck){
  			printf("State:RootStateSendAck\n");
  			printfflush();
  		}else if (currentState == BackOffState){
  			printf("State:BackOffState\n");
  			printfflush();
  		}else if (currentState == ChildStateSendSync){
  			printf("State:ChildStateSendSync\n");
  			printfflush();
  		}else if (currentState == ChildStateSendAck){
  			printf("State:ChildStateSendAck\n");
  			printfflush();
  		}*/
  	
	  	if (levelLocked == TRUE) {
		  	if (treeLevel == 0) {
		  		call Leds.led0On();
		  		call Leds.led1Off();
		  		call Leds.led2Off();
		  	} else if (treeLevel == 1) {
		  		call Leds.led0Off();
		  		call Leds.led1On();
		  		call Leds.led2Off();
		  	} else if (treeLevel == 2) {
		  		call Leds.led0Off();
		  		call Leds.led1Off();
		  		call Leds.led2On();
		  	}
	  	}		
  	}
      
  	event message_t * Receive.receive(message_t *msg, void *payload, uint8_t len) {
  		msg_received = *((Package * ) payload);
  		
    	switch (msg_received.messageType) { // Cases for the different message types received
    		case msg_LevelRequest:
    			SendMessage(msg_LevelDisc, treeLevel, 0, 0);		
    			break;
    		case msg_LevelDisc:
    			if (currentState == StartState) { // If a level has not been assigned 
	    			treeLevelReceived = msg_received.nodeLevel;
	    			call TimerLockLevel.startOneShot(LevelDiscTimeout);
	    			printf("Level Discovered at: %lu us\n", call TimeStamp.timestamp(msg));
  					printfflush();
    			}
	    		break;
	    	case msg_SyncPuls:
	    		// if the syncpulse was for this node 
	    		if ((currentState == RootState || currentState == ChildState) && (treeLevel == msg_received.nodeLevel - 1)) {
	    			// Get T2 timestamp
	    			if (call TimeStamp.isValid(msg)) {
    					T2 = call TimeStamp.timestamp(msg);
    					printf("Receive ved T2: %lu \n", call TimeStamp.timestamp(msg));
  						printfflush();
    				} 
    				
    				if (currentState == RootState) {
	    				currentState = RootStateSendAck;
	    				printf("State:ChildStateSendAck\n");
  						printfflush();
	    			} else if (currentState == ChildState) {
	    				currentState = ChildStateSendAck;
	    				printf("State:ChildStateSendAck\n");
  						printfflush();
	    			}
	    			SendMessage(msg_Ack, treeLevel, 0, 0); // Sends the Ack Message so T4 can be created	
	    		} else { // If the sync pulse was not for this node
    				if (currentState == ChildState || currentState == BackOffState) {
	    				WaitRandom(BackOffTimeLow, BackOffTimeHigh);
	    				currentState = BackOffState;
	    				printf("State:BackOffState\n");
  						printfflush(); 
	    			}
	    		}
    			break;
    		case msg_T2andT3info:
    			if (currentState == ChildStateSendSync) {
	    			// Get T2 and T3 values
	    			T2 = msg_received.t2;
	    			T3 = msg_received.t3;
	    			
	    			// Do calculations
	    			clkOffSet = ((T2-T1)-(T4-T3))/2;
					propDelay = ((T2-T1)+(T4-T3))/2;
					
	    			// printf the results
	    			printf("Level %d\n", treeLevel);
  					printf("Offset %lu us\n", clkOffSet);
  					printf("Delay %lu us\n", propDelay);
  					printfflush();
  					
	    			currentState = ChildState;
	    			printf("State:ChildState\n");
  					printfflush();
	    			//call SyncWait.stop();  			// Stops the retransmission timer
    			}
    			break;
    		case msg_Ack:
    			if (currentState == ChildStateSendSync) {
	    			if (call TimeStamp.isValid(msg)) {
    					T4 = call TimeStamp.timestamp(msg);
    					printf("Receive ved T4: %lu \n", call TimeStamp.timestamp(msg));
  						printfflush();
    				}
    			}
    			break;
    		case msg_StartSync: // Start sync message sent from the rootnode
    			currentState = ChildStateSendSync;
    			printf("State:ChildStateSendSync\n");
  				printfflush();
    			WaitRandom(WaitRandomTimeLow, WaitRandomTimeHigh);
    			printf("Receive StartSync\n");
  				printfflush();
    			break;
    		default:
    			break;
    	}
 		return msg;
  	}

	event void AMSend.sendDone(message_t *msg, error_t error) {
		printf("State: %d \n", currentState);
  		printfflush();
		if (&packet == msg) {
			locked = FALSE;
		}
		printf("State: %d \n", currentState);
  		printfflush();
		switch (currentState) {
			case ChildStateSendSync:
				// T1 = timestamp.get();
				printf("Senddone ved T1 %lu \n", call TimeStamp.timestamp(msg));
  				printfflush();
				T1 = call TimeStamp.timestamp(msg);
				break;
			case RootStateSendAck:
				//Get TimeStamp T3 
				T3 = call TimeStamp.timestamp(msg);
				// Send info with T2 and T3
				SendMessage(msg_T2andT3info, treeLevel, T2, T3);
				currentState = RootState;
				printf("State:RootState\n");
  				printfflush();
				printf("Send ved T3 %lu \n", call TimeStamp.timestamp(msg));
  				printfflush();
				break;
			default:
				break;
		}
	}
	
	event void StartTimer.fired() {
		// Start level discovery 
		if (currentState == StartState) {
			if (ROOTNODE) {
				treeLevel = 0;
				SendMessage(msg_LevelDisc, treeLevel, 0, 0);
    			levelLocked = TRUE;
				currentState = RootState;
				call SynchronizationTimer.startPeriodic(TimeSyncWait);
				printf("Is Rootnode: %lu \n", call MicroTimer.get());
  				printfflush();
			} else {
				SendMessage(msg_LevelRequest, treeLevel, 0, 0);
				call TimerLockLevel.startOneShot(LevelDiscTimeout);
			}					
		}
	}

	event void TimerLockLevel.fired() {		
		if (treeLevelReceived != 255) { // If levels has been received it assigns itself that level +1
			treeLevel = treeLevelReceived + 1;
			levelLocked = TRUE;
			treeLevelReceived = 255;
			currentState = ChildState;
			WaitRandom(0, LevelDiscWaitTime);
			printf("Is Childnode: %lu \n", call MicroTimer.get());
  			printfflush();
		} else {
			levelDiscCount++;
			if (levelDiscCount < 6) { // Asks again for level discovery if none has been received
				SendMessage(msg_LevelRequest, treeLevel, 0, 0);
				call TimerLockLevel.startOneShot(LevelDiscTimeout);
			} else {					// Has given up on level discovery
				levelDiscCount = 0;
			}
		}		
	}
	
	event void SyncWait.fired() {
		switch (currentState) {
			case ChildState: // Sends the level discovery packets for the next levels					
				SendMessage(msg_LevelDisc, treeLevel, 0, 0); 
				break;
			case ChildStateSendSync:
				SendMessage(msg_SyncPuls, treeLevel, 0, 0);		// Send Sync Pulse
				//WaitRandom(AckTimeOutLow, AckTimeOuthigh); 	// Starts timeout for retransmission	
				printf("SyncPuls Sent\n");
  				printfflush();
				break;
			case RootStateSendAck:
				SendMessage(msg_Ack, treeLevel, 0, 0);
				break;
			case ChildStateSendAck:
				SendMessage(msg_Ack, treeLevel, 0, 0);
				break;
			case BackOffState:
				SendMessage(msg_SyncPuls, treeLevel, 0, 0);
				currentState = ChildStateSendSync;
				break;
			default:
				break;
		}
	}

	event void SynchronizationTimer.fired() {
		// Send Syncronization_pulse
		SendMessage(msg_StartSync, treeLevel, 0, 0);
		printf("StartSync Send: %lu \n", call MicroTimer.get());
  		printfflush();
	}

	async event void HplAtm128Interrupt.fired() {
		call Leds.led1Toggle();
	}
}